﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace BodyTrain
{
    //提供了一些骨骼数据的常用函数
    public static class BodyExtensions
    {
        #region Public methods
        
        //返回指定骨骼的高度，单位为米，利用头部到脚部的骨骼点计算身高
        public static double Height(this Body body)
        {
            const double HEAD_DIVERGENCE = 0.11;//0.1

            var head = body.Joints[JointType.Head];             //头
            var spineShoulder = body.Joints[JointType.SpineShoulder];    //在肩膀之间的脊柱
            var spine = body.Joints[JointType.SpineMid];        //脊柱中部
            var waist = body.Joints[JointType.SpineBase];       //脊柱底部
            var hipLeft = body.Joints[JointType.HipLeft];       //左臀
            var hipRight = body.Joints[JointType.HipRight];     //右臀
            var kneeLeft = body.Joints[JointType.KneeLeft];     //左膝盖
            var kneeRight = body.Joints[JointType.KneeRight];   //右膝盖
            var ankleLeft = body.Joints[JointType.AnkleLeft];   //左脚踝
            var ankleRight = body.Joints[JointType.AnkleRight]; //右脚踝
            var footLeft = body.Joints[JointType.FootLeft];     //左脚
            var footRight = body.Joints[JointType.FootRight];   //右脚        //原码缺少neck骨骼节点
            ///////////////////////       Yan        ///////////////////////////
            var neck = body.Joints[JointType.Neck];           // neck骨骼节点

            var handTipLeft = body.Joints[JointType.HandTipLeft];   //左手指尖骨骼节点
            var handLeft = body.Joints[JointType.HandLeft];         //左手骨骼节点
            var wristLeft = body.Joints[JointType.WristLeft];         //左手腕节点
            var elbowLeft = body.Joints[JointType.ElbowLeft];         //左手肘节点
            var shoulderLeft = body.Joints[JointType.ShoulderLeft];   //左肩膀节点

            var handTipRight = body.Joints[JointType.HandTipRight];   //右手指尖骨骼节点
            var handRight = body.Joints[JointType.HandRight];         //右手骨骼节点
            var wristRight = body.Joints[JointType.WristRight];         //右手腕节点
            var elbowRight = body.Joints[JointType.ElbowRight];         //右手肘节点
            var shoulderRight = body.Joints[JointType.ShoulderRight];   //右肩膀节点

            //return (Length(handTipLeft, handLeft,wristLeft, elbowLeft, shoulderLeft,spineShoulder,shoulderRight,elbowRight,wristRight,handRight,handTipRight));   //利用臂长等于身高 by Yan
            ///////////////////////////////////////////////////////////////////

            // Find which leg is tracked more accurately.
            int legLeftTrackedJoints = NumberOfTrackedJoints(hipLeft, kneeLeft, ankleLeft, footLeft);   //在给定的骨骼点集合下，计算被准确跟踪的关节个数
            int legRightTrackedJoints = NumberOfTrackedJoints(hipRight, kneeRight, ankleRight, footRight);//臀部，膝盖，脚踝，脚

            double legLength = legLeftTrackedJoints > legRightTrackedJoints ? Length(hipLeft, kneeLeft, ankleLeft, footLeft) : Length(hipRight, kneeRight, ankleRight, footRight);

            //头+在肩膀之间的脊柱+脊柱中部+脊柱底部的长度，脚的长度，头顶部分的偏差
            return (Length(head, neck,spineShoulder, spine, waist) + legLength + HEAD_DIVERGENCE);  //修改了，增加了neck节点的考量 By Yan
        }

        //利用臂长等于身高，测量身高
        public static double ArmExtendWith(this Body body)
        {
            var armWidthDeviation = 0.5; //0.5
            var handTipRight =  body.Joints[JointType.HandTipRight]; //Alvin
            var handRight = body.Joints[JointType.HandRight];
            var wristRight = body.Joints[JointType.WristRight];
            var elowRight = body.Joints[JointType.ElbowRight];
            var shoulderRight = body.Joints[JointType.ShoulderRight];
            var shoulderCenter = body.Joints[JointType.SpineShoulder];

            var handTipLeft = body.Joints[JointType.HandTipLeft]; //Alvin
            var handleft = body.Joints[JointType.HandLeft];
            var wristleft = body.Joints[JointType.WristLeft];
            var elowleft = body.Joints[JointType.ElbowLeft];
            var shoulderleft = body.Joints[JointType.ShoulderLeft];


            // Calculate the left and right arm extends width
            double rightArmExtendsWidth = Length(handTipRight,handRight, wristRight, elowRight, shoulderRight, shoulderCenter);
            double leftArmExtendsWidth = Length(handTipLeft,handleft, wristleft, elowleft, shoulderleft, shoulderCenter);

            return rightArmExtendsWidth + leftArmExtendsWidth + armWidthDeviation;
        }

        public static double UpperHeight(this Body skeleton)
        {
            var head = skeleton.Joints[JointType.Head];
            var neck = skeleton.Joints[JointType.SpineShoulder];
            var spine = skeleton.Joints[JointType.SpineMid];
            var waist = skeleton.Joints[JointType.SpineBase];

            return Length(head, neck, spine, waist);
        }

        public static double Length(Joint p1, Joint p2)
        {
            return Math.Sqrt(
                Math.Pow(p1.Position.X - p2.Position.X, 2) +
                Math.Pow(p1.Position.Y - p2.Position.Y, 2) +
                Math.Pow(p1.Position.Z - p2.Position.Z, 2));
        }

        public static double Length(params Joint[] joints)
        {
            double length = 0;

            for (int index = 0; index < joints.Length - 1; index++)
            {
                length += Length(joints[index], joints[index + 1]);
            }

            return length;
        }

        //在给定的骨骼点集合下，计算被准确跟踪的关节个数
        public static int NumberOfTrackedJoints(params Joint[] joints)
        {
            int trackedJoints = 0;

            foreach (var joint in joints)
            {
                if (joint.TrackingState == TrackingState.Tracked)
                {
                    trackedJoints++;
                }
            }


            return trackedJoints;
        }

        public static Joint ScaleTo(this Joint joint, int width, int height, float skeletonMaxX, float skeletonMaxY)
        {
            CameraSpacePoint position = new CameraSpacePoint()
            {
                X = Scale(width, skeletonMaxX, joint.Position.X),
                Y = Scale(height, skeletonMaxY, -joint.Position.Y),
                Z = joint.Position.Z
            };

            joint.Position = position;

            return joint;
        }

        public static Joint ScaleTo(this Joint joint, int width, int height)
        {
            return ScaleTo(joint, width, height, 1.0f, 1.0f);
        }
        #endregion

        #region Helpers
        public static float Scale(int maxPixel, float maxSkeleton, float position)
        {
            float value = ((((maxPixel / maxSkeleton) / 2) * position) + (maxPixel / 2));

            if (value > maxPixel)
            {
                return maxPixel;
            }

            if (value < 0)
            {
                return 0;
            }

            return value;
        }
        #endregion
    }
}
